import math

from Event import *

class PathGenerator:
    def __init__(self):
        self.k = 80.0 # Target acceleration
        self.t0 = 0 # Time of path start
        self.angle0 = 0 # Angle at start of path
        self.angularVelocity0 = 0 # Angle at start of path
        self.tm = 0 # Time to finish half the path
        self.stopArm = True
        self.goalState = None
        self.history = None
        self.f = open("tPG.txt", "w")

    def generatePath(self, startEvent, goalEvent):
        # Todo: Take start and end angular velocity into account
        self.deltaAngle = goalEvent.state.arm1.a - startEvent.state.arm1.a
        self.t0 = startEvent.time
        self.angle0 = startEvent.state.arm1.a
        self.angularVelocity0 = startEvent.state.arm1.aDot
        self.tm = math.sqrt(math.fabs(self.deltaAngle/self.k))
        dtStop = math.fabs(self.angularVelocity0 / self.k)
        if math.fabs(startEvent.state.arm1.aDot) > 0.1 and dtStop > 0.01:
            print "Path generator: Generated stop path"
            self.tm = 0.5 * dtStop
            self.stopArm = True
        else:
            print "Path generator: Generated move to goal path"
            self.stopArm = False
        self.tm = 0.5 * dtStop
        self.stopArm = True
        self.goalState = goalEvent.state

    def accelerate(self, state, dt):
        s = State()
        s.arm1.a = state.arm1.a + state.arm1.aDot*dt + 0.5*self.k*dt*dt
        s.arm1.aDot = state.arm1.aDot + self.k*dt
        return s

    def decelerate(self, state, dt):
        s = State()
        s.arm1.a = state.arm1.a + state.arm1.aDot*dt - 0.5*self.k*dt*dt
        s.arm1.aDot = state.arm1.aDot - self.k*dt
        return s
        
    def getNextStateHistory(self, time, dt):
        if self.history == None:
            print "PathGenerator: ERROR: No history."
            exit
        s = self.history.getState(time+dt)
        self.f.write("{0} {1} {2}\n".format(time+dt, s.arm1.a, s.arm1.aDot))
        return s
        
    def getNextState(self, state, time, dt):
        return self.getNextStateHistory(time, dt)
        if state == None or self.goalState == None:
            return state
        dx = self.goalState.arm1.a - state.arm1.a
        # Check if sign of velocity is wrong
        if dx > 0 and state.arm1.aDot < 0:
            return self.accelerate(state, dt)
        if dx <= 0 and state.arm1.aDot >= 0: 
            return self.decelerate(state, dt)
        dxB = 0.5*state.arm1.aDot*state.arm1.aDot/self.k
        s = None
        if dx > 0:
            if dx > dxB:
                s = self.accelerate(state, dt)
            else:
                s = self.decelerate(state, dt)
        else:
            if math.fabs(dx) > dxB:
                s = self.decelerate(state, dt)
            else:
                s = self.accelerate(state, dt)
        self.f.write("{0} {1} {2} {3} {4}\n".format(time, s.arm1.a, s.arm1.aDot))
        return s        
                
    # def getState(self, time):
        # if time < self.t0:
            # return None
        # s = State()
        # if self.stopArm:
            # dt = time - self.t0
            # if dt > math.fabs(self.angularVelocity0 / self.k):
                # return None
            # if self.angularVelocity0 > 0:
                # a = -self.k
            # else:
                # a = self.k
            # s.arm1.a = self.angle0 + self.angularVelocity0*dt + 0.5*a*dt*dt
            # s.arm1.aDot = self.angularVelocity0 + a*dt
        # else:
            # # Check if time is up
            # #print self.t0, self.tm, time
            # if time > self.t0 + 2.0*self.tm:
                # return None
            # dt = time - self.t0
            # if dt < self.tm:
                # a = 0.5*self.k*dt*dt
                # aDot = dt*self.k
            # else:
                # a = self.k*(2.0*self.tm*dt - 0.5*dt*dt - self.tm*self.tm)
                # aDot = self.k*(2.0*self.tm - dt)
            # if self.deltaAngle > 0:
                # s.arm1.a = self.angle0 + a
            # else:
                # s.arm1.a = self.angle0 - a
            # s.arm1.aDot = aDot
        # return s
        